package Controller;

import java.util.ArrayList;
import java.util.Hashtable;

import Interface.BluetoothNXT;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;

/**
 * This class acts as a linefollower (actually edgefollower), which follows a
 * line by regulating the speed of the motors according to the readings of the
 * lightsensor. When a very sharp corner occurs, it rotates in places to get
 * aligned again.
 */
public class LineFollowerV2 {

	private static LightSensor sensor = new LightSensor(SensorPort.S1);

	private static Motor motorL = Motor.B;
	private static Motor motorR = Motor.C;

	// degree per second (dps)
	private static int motorSpeedL = 360;
	private static int motorSpeedR = 360;
	private static int defaultSpeed = 360;

	private static int maxSpeed = 500;
	private static int minSpeed = 0;

	private static int reading = 0;

	private static TachoPilot pilot = new TachoPilot(2.20f, 6.30f, motorL,
			motorR);

	public static void main(String[] args) throws Exception {

		calibrateLightSensor();

		setMaxSpeed();

		motorL.setSpeed(motorSpeedL);
		motorR.setSpeed(motorSpeedR);
		//connect to bluetooth
				LCD.drawString("Connect Bluetooth?", 0, 1);
				int button = Button.waitForPress();
				if (button == Button.ID_ENTER)
				{		
					BluetoothNXT test = new BluetoothNXT(pilot,sensor, new UltrasonicSensor(SensorPort.S2));
					test.start();
				}
				
		action();
	}

	public static void calibrateLightSensor() throws InterruptedException {

		Thread.sleep(1000);
		System.out.println("Calibrating lightsensor");
		System.out.println("Set neutral value");
		Thread.sleep(1000);
		while (!Button.ENTER.isPressed()) {
			sensor.calibrateLow();
		}
		Thread.sleep(1000);
		System.out.println("Set line value");
		while (!Button.ENTER.isPressed()) {
			sensor.calibrateHigh();
		}
		System.out.println("Lightsensor calibrated");
		System.out.println("Low = " + sensor.getLow());
		System.out.println("High = " + sensor.getHigh());
		System.out.println("Press enter to start.");
		Button.ENTER.waitForPress();
	}

	private static void setMaxSpeed() {
		LCD.clear();
		LCD.drawString("Maxspeed: " + maxSpeed + " degr/sec",0,2 );
		Buttonloop: {
			while (true) {				
				int button = Button.waitForPress();
				switch (button) {
				case 1:
					break Buttonloop;
				case 2:
					maxSpeed -= 100;
					if (maxSpeed < 100)
						maxSpeed = 100;
					break;
				case 4:
					maxSpeed += 100;
					break;				
				}
				LCD.drawString("Maxspeed: " + maxSpeed + " degr/sec",0,2 );
			}
		}
	}

	private static void action() {

		pilot.forward();

		while (true) {
			reading = sensor.readValue();

			// Motorspeed from maxSpeed dps to minSpeed dps
			motorSpeedL = (int) (maxSpeed + (-maxSpeed + minSpeed) / 100
					* Math.abs(reading));
			if (motorSpeedL > maxSpeed)
				motorSpeedL = maxSpeed;
			if (motorSpeedL < minSpeed + 50)
				motorSpeedL = minSpeed;

			motorSpeedR = (int) (minSpeed + (-minSpeed + maxSpeed) / 100
					* Math.abs(reading));
			if (motorSpeedR > maxSpeed)
				motorSpeedR = maxSpeed;
			if (motorSpeedR < minSpeed + 50)
				motorSpeedR = minSpeed;

			motorL.setSpeed(motorSpeedL);
			motorR.setSpeed(motorSpeedR);

			if (reading > 80) {
				sharpLeftTurn();
			}

			if (reading < 20) {
				sharpRightTurn();
			}

		}

	}

	private static void sharpLeftTurn() {
		System.out.println(reading + ": Left!");

		pilot.stop();
		motorL.setSpeed(defaultSpeed);
		motorR.setSpeed(defaultSpeed);

		pilot.rotate(5);

		// Thread.sleep(500);
		pilot.forward();
	}

	private static void sharpRightTurn() {
		System.out.println(reading + ": Right!");

		pilot.stop();
		motorL.setSpeed(defaultSpeed);
		motorR.setSpeed(defaultSpeed);

		pilot.rotate(-5);

		// Thread.sleep(500);
		pilot.forward();
	}
}
